#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include<pcl/filters/passthrough.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=100000;
string frame_id_string_,pc_path_name_;
ros::Publisher cloud_pub;

Eigen::Matrix4f P2_, Trv2c_;

void cloudCB(const vector<string>& label_path)
{
    Eigen::Vector4f obj_center_;

    for(int label_idx = 0; label_idx < label_path.size(); label_idx++){

        ifstream read_label;
        read_label.open(label_path[label_idx]);
        string s_line,class_,truncated_,dx_,dy_,dz_,x_,y_,z_,theta_,ahpha_;
        vector<vector<string>> label;

        while(getline(read_label,s_line)){
            istringstream sin(s_line);
            vector<string> data_line;
            string temp;
            for(int i=0;i<16;++i){
                sin>>temp;
                data_line.push_back(temp);
            }
            label.push_back(data_line);
        }
        read_label.close();
        ofstream write_label;
        write_label.open(label_path[label_idx]);
        for(auto& d_l:label){
            for(int i=0;i<d_l.size()-1;++i){
                if(d_l[i]=="Pedestrian")
                write_label<<"Car ";
                else
                write_label<<d_l[i]<<" ";
            }
            write_label<<d_l.back()<<endl;
        }
       write_label.close();
    }
}

vector<string> getFiles(string cate_dir)
{
    vector<string> files; //存放文件名

    DIR *dir;
    struct dirent *ptr;
    char base[1000];

    if ((dir = opendir(cate_dir.c_str())) == NULL)
    {
        perror("Open dir error...");
        exit(1);
    }

    while ((ptr=readdir(dir)) != NULL)
	{
		if(strcmp(ptr->d_name,".")==0 || strcmp(ptr->d_name,"..")==0)    ///current dir OR parrent dir
            continue;
        else if(ptr->d_type == 8)    ///file
			//printf("d_name:%s/%s\n",basePath,ptr->d_name);
			files.push_back(ptr->d_name);
		else if(ptr->d_type == 10)    ///link file
			//printf("d_name:%s/%s\n",basePath,ptr->d_name);
			continue;
		else if(ptr->d_type == 4)    ///dir
		{
			files.push_back(ptr->d_name);
			/*
		        memset(base,'\0',sizeof(base));
		        strcpy(base,basePath);
		        strcat(base,"/");
		        strcat(base,ptr->d_nSame);
		        readFileList(base);
			*/
		}
	}
	closedir(dir);
 
	//排序，按从小到大排序
	sort(files.begin(), files.end());
	return files;
}

int main(int argc, char* argv[])
{

    string label_path_m = "/media/st/data_fast/gazebo/0813/ciassd/my_data/training/label_2";

    vector<string> label_path = getFiles(label_path_m);
    
    for (auto &path : label_path){
        path = label_path_m+"/"+path;
        cout<<path<<endl;
    }

    cloudCB(label_path);

    return 0;
}
